#pragma once

#include "armor.hpp"

class Compensator {
 private:
  double ballet_speed_;
  cv::Mat cam_mat_, distor_coff_;

  double PinHoleEstimate(Armor& armor);
  double SolveSurfaceLanchAngle(cv::Point2f target);
  /**
   * @brief 返回补偿后的pitch
   *
   * @param distance 与目标距离
   * @param pitch robot.pitch + 不参与补偿pitch(弧度)
   * @return double 补偿后的pitch(弧度)
   */
  double SolveOffset(float distance, float pitch);

  void VisualizePnp(Armor& armor, const cv::Mat& output, bool add_lable);
  void VisualizeSolveAngle(Armor& armor, const cv::Mat& output, bool add_lable);

 public:
  Compensator();
  Compensator(const std::string& cam_mat_path);
  ~Compensator();

  cv::Point3f EstimateWorldCoord(Armor& armor, double& distance);

  void LoadCameraMat(const std::string& path);

  /**
   * @brief eulr 正值 : 向左向下
   *
   * @param armors
   * @param frame
   * @param ballet_speed 单位m/s
   */
  void Apply(std::vector<Armor>& armors, const cv::Mat& frame,
             float ballet_speed, float robot_pitch);
  void VisualizeResult(std::vector<Armor>& armors, const cv::Mat& output,
                       int verbose = 1);
};
